
/****************************************************************************
*						PlanesDetection Class header						*
*    				Copyright (C) 2012  J.R. Ruiz-Sarmiento					*
*																			*
*    This program is free software: you can redistribute it and/or modify	*
*    it under the terms of the GNU General Public License as published by	*
*    the Free Software Foundation, either version 3 of the License, or		*
*    (at your option) any later version.									*
*																			*
*    This program is distributed in the hope that it will be useful,		*
*    but WITHOUT ANY WARRANTY; without even the implied warranty of			*
*    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the			*
*    GNU General Public License for more details.							*
*																			*
*    You should have received a copy of the GNU General Public License		*
*    along with this program.  If not, see <http://www.gnu.org/licenses/>.	*
*																			*
****************************************************************************/

#ifndef SORT_PLANESDETECTION_H
#define SORT_PLANESDETECTION_H

#include <mrpt/utils.h>

#include <pcl/common/angles.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/segmentation/planar_region.h>

#include <pcl/filters/fast_bilateral.h>

#include "Models.h"


namespace SORT
{
namespace ObjectRecognition
{

	class CPlanesDetection
	{
	private:

		typedef Eigen::aligned_allocator<pcl::PlanarRegion<pcl::PointXYZ>> PlanarRegionAllocator;	//!< Typedef for a more readable code.

		pcl::PointCloud<pcl::PointXYZ>::Ptr				m_cloud;	//!< Point cloud for planes detection.
		std::vector<pcl::PlanarRegion<pcl::PointXYZ>, 
						PlanarRegionAllocator>			m_regions;				//!< Regions detected.
		pcl::PointCloud<pcl::Normal>::Ptr				m_normalCloud;			//!< Normals computed of the input point cloud.
		std::vector<pcl::ModelCoefficients>				m_modelCoefficients;	//!< Coefficients of the detected planes.
		std::vector<pcl::PointIndices>					m_inlierIndices;		//!< Vector of inliers for each plane detected.
		pcl::PointCloud<pcl::Label>::Ptr				m_labels;				//!< Labels of each plane.
		std::vector<pcl::PointIndices>					m_labelIndices;			//!< Points agrouped in each label.
		std::vector<pcl::PointIndices>					m_boundaryIndices;		//!< Boundary indices of each plane.

		// Thresholds

		size_t	m_minPlaneInliers;	//!< Minimun number of inliers to consider a region as plane.
		double	m_distThreshold;	//!< Maximum distance of the current point to the candidate plane to be considered part of it.
		double	m_angleThreshold;	
		double	m_maximumCurvature;

		size_t	m_minInliersInVerticalPlanes;
		size_t	m_minInliersInHorizontalPlanes;
		size_t	m_minInliersGeneral;

		bool	m_detectHorizontalPlanes;
		bool	m_detectVerticalPlanes;
		bool	m_detectGeneralPlanes;

		pcl::FastBilateralFilter<pcl::PointXYZ> m_bilateralFilter;

		bool	m_useBilateralFilter;

		struct bilateralFilterConf
		{
			float	sigmaS;
			float	sigmaR;
			bool	earlyDivision;	
		}m_bilateralFilterConf;

	public:

		static bool DEBUGGING_PLANES_DETECTION;

		/** Constructor.
		  */
		CPlanesDetection();

		/** Destructor.
		  */
		~CPlanesDetection(){};

		/** Set the input cloud where search planes.
		  * \param cloud Cloud to set as input.
		  */
		inline void setInputCloud( pcl::PointCloud<pcl::PointXYZ>::Ptr cloud ){ m_cloud = cloud; }


		inline void setMinimunPlaneInliers( const size_t &N ){ m_minPlaneInliers = N; }
		inline void setMinimunInliersInVerticalPlanes( const size_t &N ){ m_minInliersInVerticalPlanes = N; }
		inline void setMinimunInliersInHorizontalPlanes( const size_t &N ){ m_minInliersInHorizontalPlanes = N; }
		inline void setMinimunInliersInGeneral( const size_t &N ){ m_minInliersGeneral = N; }
		
		/** Set the used distance threshold.
		  * \param distThreshold New threshold value.
		  */
		inline void setDistThreshold( const double &distThreshold ) { m_distThreshold = distThreshold; }

		/** Set the used angle threshold.
		  * \param angleThreshold New threshold value.
		  */
		inline void setAngleThreshold( const double &angleThreshold ) { m_angleThreshold = angleThreshold; }

		/** Set the used maximum curvature threshold.
		  * \param maximumCurvature New threshold value.
		  */
		inline void setMaximumCurvature( const double &maximumCurvature ) { m_maximumCurvature = maximumCurvature; }

		inline void setDetectHorizontalPlanes( const bool &B ){ m_detectHorizontalPlanes = B; }
		inline void setDetectVerticalPlanes( const bool &B ){ m_detectVerticalPlanes = B; }
		inline void setDetectGeneralPlanes( const bool &B ){ m_detectGeneralPlanes = B; }

		/** Detect planes using the RANSAC implementation of PCL.
		  * \param planes Vector of planes detected as CPlane objects.
		  */
		void detectPlanes( mrpt::aligned_containers<CPlane>::vector_t &planes );

		/** Detect planes using OrganizedMultiplaneSegmentation from PCL. If m_useBilateralFilter,
		  *	then a bilateral filter is applied before the segmentation.
		  */
		void detectPlanes();

		/** Get the content of member variables computed into a planesDetection process.
 		  */
		inline void getRegions (std::vector<pcl::PlanarRegion<pcl::PointXYZ>,PlanarRegionAllocator> &regions ) const { regions = m_regions; }
		inline void getNormalCloud( pcl::PointCloud<pcl::Normal>::Ptr &normalCloud ) const { normalCloud = m_normalCloud; }
		inline void getModelCoefficients( std::vector<pcl::ModelCoefficients> &modelCoeffs ) const { modelCoeffs = m_modelCoefficients; }
		inline void getInlierIndices( std::vector<pcl::PointIndices>	&inlierIndices ) const { inlierIndices = m_inlierIndices; }
		inline void getLabels( pcl::PointCloud<pcl::Label>::Ptr labels ) const { labels = m_labels; }
		inline void getLabelIndices( std::vector<pcl::PointIndices> &labelIndices ) const { labelIndices = m_labelIndices; }
		inline void getBoundaryIndices( std::vector<pcl::PointIndices> &boundaryIndices ) const { boundaryIndices = m_boundaryIndices; }
		
		/** Get the detected planes after a detectPlanes execution as point clouds.
		  * \param v_planes An empty vector of point clouds.
		  */
		void getDetectedPlanesAsPointClouds( vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &v_planes );

		/** Get the detected planes and their normals after a detectPlanes execution as point clouds.
		  * \param v_planes An empty vector of point clouds.
		  * \param v_normals An empty vector of normals.
		  */
		void getDetectedPlanesAndNormalsAsPointClouds( vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &v_planes, vector<pcl::PointCloud<pcl::Normal>::Ptr> &v_normals  );

		/** Get the detected planes after a detectPlanes execution as CPlanes.
		  * \param v_planes An empty vector of CPlanes.
		  */
		void getDetectedPlanesAsCPlanes( mrpt::aligned_containers<CPlane>::vector_t &planes );

		/** Method uses while debugging. Requests to the user for configuration params introduced by the keyboard.
		  */
		void requestConfigParams();

	private:

		/** Detect planes using the RANSAC algorithm implemented in PCL.
		  * \param planes Vector of detected planes as CPlane objects. 
		  * \param indices Indices to avoid (not to take into account) in the planes detection.
		  * \param type	Type of plane to detect.
          */
		void detectPlanes( mrpt::aligned_containers<CPlane>::vector_t &planes, 
							std::vector<int> &indices, 
							const CPlane::TPlaneType &type );

	public:
		EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	
	};

	#define IF_DEBUGGING if ( DEBUGGING_PLANES_DETECTION )
}
}

#endif
/*
	EuclideanClusterComparator

    euclidean_cluster_comparator_->setInputCloud (cloud);
    euclidean_cluster_comparator_->setLabels (labels);
    euclidean_cluster_comparator_->setExcludeLabels (plane_labels);
    euclidean_cluster_comparator_->setDistanceThreshold (0.01f, false);

    pcl::PointCloud<pcl::Label> euclidean_labels;
    std::vector<pcl::PointIndices> euclidean_label_indices;
    pcl::OrganizedConnectedComponentSegmentation<PointT,pcl::Label> euclidean_segmentation (euclidean_cluster_comparator_);
    euclidean_segmentation.setInputCloud (cloud);
    euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);
    
    for (size_t i = 0; i < euclidean_label_indices.size (); i++)
    {
      if (euclidean_label_indices[i].indices.size () > 1000)
      {
        pcl::PointCloud<PointT> cluster;
        pcl::copyPointCloud (*cloud,euclidean_label_indices[i].indices,cluster);
        clusters.push_back (cluster);
      }    
    }


	  Eigen::Vector4f model = region.getCoefficients ();
  pcl::PointCloud<PointT> poly;
  poly.points = region.getContour ();
  
  for (size_t i = 0; i < cluster.points.size (); i++)
  {
    double ptp_dist = fabs (model[0] * cluster.points[i].x +
                            model[1] * cluster.points[i].y +
                            model[2] * cluster.points[i].z +
                            model[3]);
    bool in_poly = pcl::isPointIn2DPolygon<PointT> (cluster.points[i], poly);
    if (in_poly && ptp_dist < 0.02)
      return true;
  }
  return false;
  */